import numpy as np
import cv2

cap = cv2.VideoCapture('./vtest.avi')

# params for ShiTomasi corner detection
feature_params = dict(maxCorners=100,
                      qualityLevel=0.3,
                      minDistance=7,
                      blockSize=7)

# Parameters for lucas kanade optical flow
lk_params = dict(winSize=(15, 15),
                 maxLevel=2,
                 criteria=(
                     cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03))

# Create some random colors
# color = np.random.randint(0, 255, (100, 3))

# Take first frame and find corners in it
ret, old_frame = cap.read()
old_gray = cv2.cvtColor(old_frame, cv2.COLOR_BGR2GRAY)
p0 = cv2.goodFeaturesToTrack(old_gray, mask=None, **feature_params)

# Create a mask image for drawing purposes
mask = np.zeros_like(old_frame)

while ret:
    ret, frame = cap.read()
    mask = np.zeros_like(old_frame)
    if frame is None:
        break
    frame_gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

    if len(p0) < 10:  # 如果老的特征点少于10个，重新获取特征点
        p0 = cv2.goodFeaturesToTrack(old_gray, mask=None, **feature_params)

    # calculate optical flow
    p1, st, err = cv2.calcOpticalFlowPyrLK(old_gray, frame_gray, p0, None,
                                           **lk_params)

    good_new = []
    good_old = []
    count = 0
    for i in range(len(p1)):
        dist = np.linalg.norm(p1[i] - p0[i])  # 算两个特征点之间的欧氏距离
        # 把符合条件的特征点取出
        if st[i] and dist >= 2.0 and dist <= 20.0:
            good_new.append(p1[i])
            good_old.append(p0[i])

    # draw the tracks
    for i, (new, old) in enumerate(zip(good_new, good_old)):
        a, b = new.ravel()
        c, d = old.ravel()
        mask = cv2.line(mask, (a, b), (c, d), (0, 0, 255), 2)
        frame = cv2.circle(frame, (a, b), 3, (0, 255, 0), -1)
    img = cv2.add(frame, mask)  # 图像叠加

    cv2.imshow('frame', img)
    if cv2.waitKey(100) & 0xff == ord("q"):
        break

    # Now update the previous frame and previous points
    old_gray = frame_gray.copy()
    p0 = np.array(good_new).reshape(-1, 1, 2)

cv2.destroyAllWindows()
cap.release()
